#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     HTAC,                sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "HTAC-driver.h"
#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  ClearTimer(T1);
  ClearTimer(T2);
  servoChangeRate[servo1] = 3;
  servoChangeRate[servo6] = 3;
  servoChangeRate[servo3] = 3;
  servoChangeRate[servo4] = 3;
  servoChangeRate[servo5] = 3;
  nPidUpdateInterval = 10;
  bFloatDuringInactiveMotorPWM = false;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}
task main()
{

  initializeRobot();
  waitForStart();   // wait for start of tele-op phase
  servo[servo1] = 255;
  nMotorEncoder[motorA] = 0;
  nMotorEncoderTarget[motorA] = 90;
  motor[motorA] = 20;
  while (nMotorRunState[motorA] != runStateIdle);
  motor[motorA] = 0;
  nMotorEncoder[motorA] = 0;
  while (true)
  {
    getJoystickSettings(joystick);
    if (joy1Btn(1) == 0 && joy1Btn(2) == 0 && joy1Btn(3) == 0 && joy1Btn(4) == 0)// robot driving code
    {
	    if (joy1Btn(8) == 0 && joystick.joy1_TopHat == -1)
      {
        getJoystickSettings(joystick);
        if (joystick.joy1_y1 > 15 || joystick.joy1_y1 < -15)
        {
          motor[motorE] = -joystick.joy1_y1;
        }
        else
        {
          motor[motorE] = 0;
        }
        if (joystick.joy1_y2 > 15 || joystick.joy1_y2 < -15)
        {
          motor[motorD] = -joystick.joy1_y2;
        }
        else
        {
          motor[motorD] = 0;
        }
      }
      if (joy1Btn(8) == 1 && joystick.joy1_TopHat == -1)
      {
        getJoystickSettings(joystick);
        if (joystick.joy1_y1 > 15 || joystick.joy1_y1 < -15)
        {
          motor[motorE] = -joystick.joy1_y1/4;
          }
        else
        {
          motor[motorE] = 0;
        }
        if (joystick.joy1_y2 > 15 || joystick.joy1_y2 < -15)
        {
          motor[motorD] = -joystick.joy1_y2/4;
        }
        else
        {
          motor[motorD] = 0;
        }
      }
    }
  int drivebackward = 0;
    getJoystickSettings(joystick);
      if (joystick.joy2_TopHat == 0)//robot baton dispensing code
      {
        motor[motorA] = 100;
        ClearTimer(T3);
      }
      if (joystick.joy2_TopHat == 4)
      {
        motor[motorA] = -100;
        ClearTimer(T3);
      }
      if (joystick.joy2_TopHat == -1)
      {
        if (time1[T3] > 2000 && nMotorEncoder[motorA] != 0)
        {
          motor[motorA] = -nMotorEncoder[motorA];
        }
        else
        {
          motor[motorA] = 0;
        }
      }
      getJoystickSettings(joystick);
      if (joy2Btn(1) == 1)// arm height code. 1 for low, 2 for medium, 3 for high, 4 for stowaway
      {
        servo[servo1] = 125;
        servo[servo6] = 35;
        servo[servo3] = 220;
      }
      if (joy2Btn(2) == 1)
      {
        servo[servo1] = 150;
        servo[servo6] = 55;
        servo[servo3] = 200;
      }
      if (joy2Btn(3) == 1)
      {
        servo[servo1] = 170;
        servo[servo6] = 73;
        servo[servo3] = 180;
      }
      if (joy2Btn(4) == 1)
      {
        motor[motorA] = -100;
        wait1Msec(100);
        motor[motorA] = 0;
        wait1Msec(100);
        servo[servo1] = 255;
        servo[servo6] = 75;
        servo[servo3] = 180;
        ClearTimer(T3);
      }
      if (joy2Btn(7) == 1)// scoop control
      {
        servo[servo6] = 20;
        servo[servo3] = 235;
      }
      if (joy2Btn(5) == 1)
      {
        servo[servo6] = 85;
        servo[servo3] = 170;
      }
      if (joy1Btn(7) == 1 && joy2Btn(7) == 0 && joy2Btn(5) == 0)
      {
        servo[servo6] = 20;
        servo[servo3] = 235;
      }
      if (joy1Btn(5) == 1 && joy2Btn(7) == 0 && joy2Btn(5) == 0)
      {
        servo[servo6] = 85;
        servo[servo3] = 170;
      }

      if (joystick.joy1_TopHat == 0)// driving twitch controls
      {
        motor[motorD] = -20;
        motor[motorE] = -20;
        wait1Msec(50);
        motor[motorD] = 0;
        motor[motorE] = 0;
        while (joystick.joy1_TopHat != -1)
        {
          wait1Msec(1);
          getJoystickSettings(joystick);
        }
      }
      if (joystick.joy1_TopHat == 2)
      {
        motor[motorD] = 20;
        wait1Msec(50);
        motor[motorD] = 0;
        motor[motorE] = 0;
        while (joystick.joy1_TopHat != -1)
        {
          wait1Msec(1);
          getJoystickSettings(joystick);
        }
      }
      if (joystick.joy1_TopHat == 4)
      {
        motor[motorD] = 20;
        motor[motorE] = 20;
        wait1Msec(50);
        motor[motorD] = 0;
        motor[motorE] = 0;
        while (joystick.joy1_TopHat != -1)
        {
          wait1Msec(1);
          getJoystickSettings(joystick);
        }
      }
      if (joystick.joy1_TopHat == 6)
      {
        motor[motorE] = 20;
        wait1Msec(50);
        motor[motorD] = 0;
        motor[motorE] = 0;
        while (joystick.joy1_TopHat != -1)
        {
          wait1Msec(1);
          getJoystickSettings(joystick);
        }
      }
      getJoystickSettings(joystick);
      if (joy1Btn(1) == 1)// driving slow controls
      {
        motor[motorD] = -25;
        motor[motorE] = 25;
      }
      if (joy1Btn(2) == 1)
      {
        motor[motorD] = 80;
        motor[motorE] = 80;
      }
      if (joy1Btn(3) == 1)
      {
        motor[motorE] = -25;
        motor[motorD] = 25;
      }
      if (joy1Btn(4) == 1)
      {
        motor[motorD] = -80;
        motor[motorE] = -80;
      }
      if (servo[servo6] > 74)// arm safeguard, prevents collisions between arm and scoop
      {
        servo[servo1] = 255;
      }
      if (joy2Btn(8) == 1)// lexan bottom of scoop control
      {
        servo[servo5] = 90;
        servo[servo4] = 175;
      }
      else
      {
        servo[servo5] = 10;
        servo[servo4] = 245;
      }
      if (joy1Btn(6) == 1)// stowaway button for driver
      {
        servo[servo1] = 240;
        servo[servo6] = 160;
        servo[servo3] = 95;
      }
      if (joy1Btn(9) == 1 || joy2Btn(9) == 1 || joy2Btn(10) == 1)// emergency baton release button
      {
        servoChangeRate[servo1] = 10;
			  servoChangeRate[servo6] = 10;
			  servoChangeRate[servo3] = 10;
			  servoChangeRate[servo4] = 10;
			  servoChangeRate[servo5] = 10;
        servo[servo5] = 90;
        servo[servo4] = 165;
        servo[servo1] = 255;
        servo[servo6] = 75;
        servo[servo3] = 180;
      }
      wait1Msec(10);
  }
}
